#!/usr/bin/env python3
# -*- coding:utf-8 -*-      

import rospy 
import numpy as np
from radio_serial.msg import Cmd_send, Uav_state, FWstates, Leaderstates

STATE_UAV = 101
CMD_UAV = 102
ARM = 1
TAKEOFF = 2
LAND = 3
POS = 4
VEL = 5
ACC = 6
POS_VEL = 7
POS_ACC = 8
VEL_ACC = 9
POS_VEL_ACC = 10

# vir_leader_state = Leaderstates()
plane1_state = FWstates()
plane2_state = FWstates()
plane3_state = FWstates()

leader_id = "plane_0"
    
def plane1_cb(msg:Uav_state):
    global plane1_state
    # 这里接收到的信息是邻居节点的二维位置和速度信息
    #TODO 后面可以在这里加上滤波
    plane1_state.ned_pos_x = msg.local_x
    plane1_state.ned_pos_y = msg.local_y
    
    plane1_state.ned_vel_x = msg.local_lin_x
    plane1_state.ned_vel_y = msg.local_lin_y

    plane1_state.yaw_angle = msg.deg_compass
    
    pub_plane_state()
    
def plane2_cb(msg:Uav_state):
    global plane2_state
    # 这里接收到的信息是邻居节点的二维位置和速度信息
    #TODO 后面可以在这里加上滤波
    plane2_state.ned_pos_x = msg.local_x
    plane2_state.ned_pos_y = msg.local_y
    plane2_state.ned_pos_z = msg.local_z
    
    plane2_state.ned_vel_x = msg.local_lin_x
    plane2_state.ned_vel_y = msg.local_lin_y
    plane2_state.ned_vel_z = msg.local_lin_z

    plane2_state.yaw_angle = msg.deg_compass
    
    pub_plane_state()
    
def plane3_cb(msg:Uav_state):
    global plane3_state
    # 这里接收到的信息是邻居节点的二维位置和速度信息
    #TODO 后面可以在这里加上滤波s
    plane3_state.ned_pos_x = msg.local_x
    plane3_state.ned_pos_y = msg.local_y
    plane3_state.ned_pos_z = msg.local_z
    
    plane3_state.ned_vel_x = msg.local_lin_x
    plane3_state.ned_vel_y = msg.local_lin_y
    plane3_state.ned_vel_z = msg.local_lin_z

    plane3_state.yaw_angle = msg.deg_compass
    
    pub_plane_state()

def pub_plane_state():
	global plane1_state
	try:
		# plane1_state_pub.publish(plane1_state)
		plane2_state_pub.publish(plane2_state)
		plane3_state_pub.publish(plane3_state)
		rospy.loginfo("Publishing neighbour states\n")

	except Exception as e:
		print(e)
  

###########################################################################################
# main process
if __name__ == '__main__':
	rospy.init_node('pack_px4_to_serial', anonymous=True)
	rospy.loginfo("sky node has been started!")
	rate = rospy.Rate(50)
 
	# cmd_sub = rospy.Subscriber("/outdoor/uav/cmd", Cmd_send, callback=cmd_cb, queue_size=1)
	cmd_sub = rospy.Subscriber("/outdoor/uav1/state", Uav_state, callback=plane1_cb, queue_size=1)
	cmd_sub = rospy.Subscriber("/outdoor/uav2/state", Uav_state, callback=plane2_cb, queue_size=1)
	cmd_sub = rospy.Subscriber("/outdoor/uav3/state", Uav_state, callback=plane3_cb, queue_size=1)
    # 这里根据飞机的编号注释掉对应发布的状态，避免覆盖飞机自生的最新状态
	# plane1_state_pub = rospy.Publisher("plane_1" + "/fixed_wing_control/fw_state", FWstates, queue_size=1)
	plane2_state_pub = rospy.Publisher("plane_2" + "/fixed_wing_control/fw_state", FWstates, queue_size=1)
	plane3_state_pub = rospy.Publisher("plane_3" + "/fixed_wing_control/fw_state", FWstates, queue_size=1)
 
	rospy.spin() #ensure the callback function can refresh immediately when the topic has a new message.
